import time
import cv2
import numpy as np
import os
# import openpyxl

from utils.detection_3d.calculate_utils import *

RADIO = 80
calculate_cfg = "utils/detection_3d/calculate_cfg.json"
# calculate_cfg = "./calculate_cfg.json"
with open(calculate_cfg, 'r') as f:
    config = json.load(f)
    CAMERA_LENGTH = float(config['x_step_side'])
    CAMERA_WIDTH = float(config['y_step_side'])
    STD_4_3 = str(config['std-4-3'])
    STD_4_4 = str(config['std-4-4'])
    STD_4_5 = str(config['std-4-5'])

global_cfg = "./global_cfg.json"
with open(global_cfg, 'r') as f:
    config = json.load(f)
    AREA_43 = config['std-43-area']
    AREA_44 = config['std-44-area']
    AREA_45 = config['std-45-area']

CAMERA_HEIGHT = 1  # 线扫单位高度 （毫米）
V_RADIO = 1000 * 1000 * 1  # 转化为立方毫米的比例
CONSTANT = CAMERA_WIDTH * CAMERA_HEIGHT * CAMERA_LENGTH / V_RADIO


def show_3d(npy, window_name="3d"):
    """
    useful
    3d显示
    Args:
        npy:
        window_name:

    Returns:

    """
    xyz_points = parse_npy_(npy)  # 得到x，y，z坐标
    x_points, y_points, z_points = xyz_points
    o3d.visualization.draw_geometries([numpy2pcd(x_points, y_points, z_points)], window_name=window_name)


def clean_corner(npy, r=30, c=45):
    """
    useful
    将角落值置0，小于0的值置0
    Args:
        npy:
        r: 行数范围
        c: 列数范围

    Returns:

    """
    # 角落置0
    row, column = npy.shape
    data = npy.copy()
    data[0:r, 0:c] = 0
    data[0:r, column - c:column] = 0
    data[row - r:row, 0:c] = 0
    data[row - r:row, column - c:column] = 0
    # 小于0置0
    index = data < 0
    index_nan = np.isnan(data)
    ind = np.logical_or(index, index_nan)
    data[ind] = 0
    return data


def clean_light(img_light_pth):
    """
    亮度图清洗，去除黑色部分，只保留工件
    必须传入包括文件名的文件路径
    Args:
        img_name: image name with file path
        imshow:
        imsave:
        target_folder: folder to save image

    Returns:
        rgb image
    """
    img_light = cv2.imread(img_light_pth)
    if img_light_pth.count("_side_") > 0:
        img_light = img_light[:2900, :1000, :]
    else:
        img_light = img_light[:1700, :]
    """
    clean pic
    """
    img_gray = cv2.cvtColor(img_light, cv2.COLOR_BGR2GRAY)
    row, column = img_gray.shape
    left = 0
    right = 0
    for i in range(row):
        if np.sum(img_gray[i, :]) != 0:
            left = i
            break
    for i in range(row - 1, -1, -1):
        if np.sum(img_gray[i, :]) != 0:
            right = i + 1
            break
    img_gray = img_gray[left:right, :]
    img_light = img_light[left:right, :, :]
    row, column = img_gray.shape
    for i in range(column):
        if np.sum(img_gray[:, i]) != 0:
            left = i
            break
    for i in range(column - 1, -1, -1):
        if np.sum(img_gray[:, i]) != 0:
            right = i + 1
            break
    img_light = img_light[:, left:right, :]
    # print(img_light.shape)
    return img_light


def clean_height(npy_height_pth, nan_num=np.nan):
    """
    useful
    Auth: WZW
    根据nan，提取周围部分，并将剩余的nan数据转为nan_num
    UpArea: [ :1500, : ]
    SideArea: [ :2900, :1000]
    PreVersion: clean_height in V1
    Args:
        image: np.array
        nan_num:
    Returns:
        compressed data
    """
    npy_height = np.load(npy_height_pth)
    if npy_height_pth.count("_side_") > 0:
        image = npy_height[:2900, :1000]
    else:
        image = npy_height[:1700, :]
    image = clean_edge(image)
    # print(image.shape)
    if not np.isnan(nan_num):
        print("数据压缩，修改nan为：" + str(nan_num))
        nan_index = np.isnan(image)
        image[nan_index] = nan_num
    npy_height = image
    return npy_height


def clean_edge(npy):
    """
    useful
    清理图像边缘
    Args:
        npy:

    Returns:
    """
    left = 0
    right = 0
    row, column = npy.shape
    for i in range(row):
        if len(npy[i, np.isnan(npy[i, :])]) != column:
            left = i
            break
    for i in range(row - 1, -1, -1):
        if len(npy[i, np.isnan(npy[i, :])]) != column:
            right = i + 1
            break
    npy = npy[left:right, :]
    row, column = npy.shape
    left = 0
    right = 0
    for i in range(column):
        if len(npy[np.isnan(npy[:, i]), i]) != row:
            left = i
            break
    for i in range(column - 1, -1, -1):
        if len(npy[np.isnan(npy[:, i]), i]) != row:
            right = i + 1
            break
    npy = npy[:, left:right]
    return npy


def parse_npy_(z_points):
    """
    useful
    产生x，y，z坐标，用于open3d
    Args:
        z_points:

    Returns:

    """

    y_num, x_num = z_points.shape

    unit_x_point = np.arange(x_num)
    x_points = np.tile(unit_x_point, y_num) / RADIO

    unit_y_point = np.arange(y_num)
    y_points = np.repeat(unit_y_point, x_num) / RADIO

    z_points = z_points.reshape((-1))

    nan_index = np.isnan(z_points)

    # remove nan points
    x_points = x_points[~nan_index]
    y_points = y_points[~nan_index]
    z_points = z_points[~nan_index]

    return (x_points, y_points, z_points)


def binary_set(npy, threshold=0.23):
    """
    useful
    Args:
        npy:
        threshold:

    Returns:

    """
    zero_index = npy < threshold
    one_index = npy >= threshold
    npy[zero_index] = 0
    npy[one_index] = 1
    return npy


def get_absolute_distance(npy_1, npy_2):
    """
    useful
    差值绝对值求和
    Args:
        npy_1:
        npy_2:

    Returns:

    """
    nan_ind1 = np.isnan(npy_1)
    nan_ind2 = np.isnan(npy_2)
    data1 = npy_1.copy()
    data2 = npy_2.copy()
    data1[nan_ind1] = 0
    data2[nan_ind2] = 0
    data = data1 - data2
    # data = npy_1 - npy_2
    data = np.absolute(data)
    return np.nansum(data)


def move_window(std_npy, input_npy, start_point, area=50, step=3):
    """
    useful
    滑窗寻找区域
    Args:
        std_npy: 标准区域
        input_npy: 输入区域
        start_point: 区域起点，矩形左上角点 (y, x)
        window_size: 区域大小 (a, b)
        area: 偏移范围 [y - area : y + area + step, x - area : x + a + step]
        step: 偏移补偿

    Returns:

    """
    row, column = std_npy.shape
    mini_result = 9999999
    x = -1
    y = -1
    for i in range(max(0, start_point[0] - area), min(input_npy.shape[0], start_point[0] + area + step), step):
        for j in range(max(0, start_point[1] - area), min(input_npy.shape[1], start_point[1] + area + step), step):
            data = clean_corner(input_npy[i:i + row, j:j + column], 0, 0)
            result = get_absolute_distance(std_npy, data)
            if result < mini_result:
                mini_result = result
                x = j
                y = i
    diff_result = mini_result * CONSTANT
    return diff_result, y, x


def dfs_count_points(dfs_data, dfs_flag, dfs_x, dfs_y, count):
    """
    useful
    Args:
        dfs_data:
        dfs_flag:
        dfs_x:
        dfs_y:
        count:

    Returns:

    """
    x, y = dfs_data.shape
    dfs_stack = []
    dfs_stack.append((dfs_x, dfs_y))
    while len(dfs_stack) > 0:
        index = dfs_stack[-1]
        dfs_stack.pop()
        if dfs_flag[index[0], index[1]] == 1:
            dfs_flag[index[0], index[1]] = 0
        else:
            continue
        if dfs_data[index[0], index[1]] == 1:
            count = count + 1
            if index[0] > 1 and dfs_flag[index[0] - 1, index[1]] == 1 and dfs_data[index[0] - 1, index[1]] == 1:
                dfs_stack.append((index[0] - 1, index[1]))
            if index[0] + 1 < x and dfs_flag[index[0] + 1, index[1]] == 1 and dfs_data[index[0] + 1, index[1]] == 1:
                dfs_stack.append((index[0] + 1, index[1]))
            if index[1] > 1 and dfs_flag[index[0], index[1] - 1] == 1 and dfs_data[index[0], index[1] - 1] == 1:
                dfs_stack.append((index[0], index[1] - 1))
            if index[1] + 1 < y and dfs_flag[index[0], index[1] + 1] == 1 and dfs_data[index[0], index[1] + 1] == 1:
                dfs_stack.append((index[0], index[1] + 1))
    return count, dfs_flag


def dfs_points_mask(dfs_data, dfs_flag, dfs_x, dfs_y, dfs_mask):
    """
    useful
    Args:
        dfs_data:
        dfs_flag:
        dfs_x:
        dfs_y:
        dfs_mask:

    Returns:

    """
    x, y = dfs_data.shape
    dfs_stack = []
    dfs_stack.append((dfs_x, dfs_y))
    while len(dfs_stack) > 0:
        index = dfs_stack[-1]
        dfs_stack.pop()
        if dfs_flag[index[0], index[1]] == 1:
            dfs_flag[index[0], index[1]] = 0
            dfs_mask[index[0], index[1]] = 0
        else:
            continue
        if dfs_data[index[0], index[1]] == 1:
            if index[0] > 1 and dfs_flag[index[0] - 1, index[1]] == 1 and dfs_data[index[0] - 1, index[1]] == 1:
                dfs_stack.append((index[0] - 1, index[1]))
            if index[0] + 1 < x and dfs_flag[index[0] + 1, index[1]] == 1 and dfs_data[index[0] + 1, index[1]] == 1:
                dfs_stack.append((index[0] + 1, index[1]))
            if index[1] > 1 and dfs_flag[index[0], index[1] - 1] == 1 and dfs_data[index[0], index[1] - 1] == 1:
                dfs_stack.append((index[0], index[1] - 1))
            if index[1] + 1 < y and dfs_flag[index[0], index[1] + 1] == 1 and dfs_data[index[0], index[1] + 1] == 1:
                dfs_stack.append((index[0], index[1] + 1))
    return dfs_mask, dfs_flag


def dfs_points_mask_left_biggest(dfs_data, dfs_flag, dfs_x, dfs_y, dfs_mask):
    """
    useful
    Args:
        dfs_data:
        dfs_flag:
        dfs_x:
        dfs_y:
        dfs_mask:

    Returns:

    """
    x, y = dfs_data.shape
    dfs_stack = []
    dfs_stack.append((dfs_x, dfs_y))
    while len(dfs_stack) > 0:
        index = dfs_stack[-1]
        dfs_stack.pop()
        if dfs_flag[index[0], index[1]] == 1:
            dfs_flag[index[0], index[1]] = 0
            dfs_mask[index[0], index[1]] = 1
        else:
            continue
        if dfs_data[index[0], index[1]] == 1:
            if index[0] > 1 and dfs_flag[index[0] - 1, index[1]] == 1 and dfs_data[index[0] - 1, index[1]] == 1:
                dfs_stack.append((index[0] - 1, index[1]))
            if index[0] + 1 < x and dfs_flag[index[0] + 1, index[1]] == 1 and dfs_data[index[0] + 1, index[1]] == 1:
                dfs_stack.append((index[0] + 1, index[1]))
            if index[1] > 1 and dfs_flag[index[0], index[1] - 1] == 1 and dfs_data[index[0], index[1] - 1] == 1:
                dfs_stack.append((index[0], index[1] - 1))
            if index[1] + 1 < y and dfs_flag[index[0], index[1] + 1] == 1 and dfs_data[index[0], index[1] + 1] == 1:
                dfs_stack.append((index[0], index[1] + 1))
    return dfs_mask, dfs_flag


def dfs_biggest_area_left(area_list, binary_data):
    """
    useful
    Args:
        area_list:
        binary_data:

    Returns:

    """
    if len(area_list) > 0:
        area_list.sort(reverse=True)
        search_flag = binary_data.copy()
        useless_mask = np.zeros(binary_data.shape)
        useless_mask, search_flag = dfs_points_mask_left_biggest(binary_data, search_flag, area_list[0][1],
                                                                 area_list[0][2], useless_mask)
        binary_data = binary_data * useless_mask
    return binary_data


def dfs_count_in_binary_map(binary_data, threshold, area):
    """
    useful
    Args:
        binary_data:
        threshold:
        area:

    Returns:

    """
    search_flag = binary_data.copy()
    useless_mask = np.ones(binary_data.shape)
    area_44 = []
    while np.sum(search_flag) > 0:
        flag_res = np.where(search_flag > 0)
        dfs_x = flag_res[0][0]
        dfs_y = flag_res[1][0]
        mask_flag = search_flag.copy()
        count, search_flag = dfs_count_points(binary_data, search_flag, dfs_x, dfs_y, 0)
        area_44.append((count, dfs_x, dfs_y))
        if count < threshold:
            useless_mask, search_flag = dfs_points_mask(binary_data, mask_flag, dfs_x, dfs_y, useless_mask)
            binary_data = binary_data * useless_mask
    if area == "44":
        binary_data = dfs_biggest_area_left(area_44, binary_data)
    return binary_data


def calculate_absolute_tin_diff_clean_nan_with_mask(std_npy, data, mask, area, origin=None, if_show=True,
                                                    target_folder=None, filename=None):
    """
    useful
    差值二值化，清除nan对应区域,可视化
    Args:
        std_npy:
        origin: 用于png黑色像素除nan，可不使用
        if_show:
    Returns:

    """
    # print("nan num: ", np.sum(origin))
    # show_3d(data)
    nan_ind1 = np.isnan(data)
    nan_ind2 = np.isnan(std_npy)
    data1 = data.copy()
    data2 = std_npy.copy()
    data1[nan_ind1] = 0
    data2[nan_ind2] = 0
    resul = np.absolute(data1 - data2)
    resul[~mask] = 0
    # show_3d(resul)
    resul = binary_set(resul)
    # show_3d(resul)
    resul = dfs_count_in_binary_map(resul, 30, area)
    # show_3d(resul)
    if not (filename is None or area is None or target_folder is None):
        draw_binary_pic_with_mask(resul, target_folder, filename, area)
    # resul = clean_nan(origin, resul)
    result = np.sum(resul)
    if if_show:
        show_3d(resul)
    return result


def draw_binary_pic_with_mask(data, target_folder, file, area):
    """
    useful
    Args:
        data:
        target_folder:
        file:
        area:

    Returns:

    """
    min_val = data.min()
    max_val = data.max()
    data = (data - min_val) * (255 / (max_val - min_val))
    data = data.astype(np.uint8)
    ret, data = cv2.threshold(data, 50, 255, cv2.THRESH_BINARY)
    cv2.imwrite(
        os.path.join(target_folder, file[:-4] + area + ".png"), data)


def calculator_tin_diff_with_mask(x_points, y_points, z_points, std_npy, npy, start_point, mask, area, pic=None,
                                  target_folder=None, filename=None):
    """
    useful
    利用mask过滤
    Args:
        x_points:
        y_points:
        z_points:
        std_npy:
        npy:
        start_point:

    Returns:

    """
    # origin_nan = np.isnan(npy)
    # 计算底面
    bottom_down, bottom_up, bottom_left = get_bottom_value(x_points, y_points, z_points)
    Z_BOTTOM = (bottom_down + bottom_up + bottom_left) / 3  # PCD面的大致底面高度
    x_points, y_points, z_points = data_filter_by_area(x_points, y_points, z_points, x_min=start_point[1] / RADIO,
                                                       x_max=(start_point[1] + std_npy.shape[1]) / RADIO,
                                                       y_min=start_point[0] / RADIO,
                                                       y_max=(start_point[0] + std_npy.shape[0]) / RADIO)
    real_bottom = get_real_bottom(Z_BOTTOM, z_points)

    # 滑窗计算结果
    result, y, x = move_window(std_npy, npy - real_bottom, start_point)
    # print("result: ", str(result), " (", str(x), ", ", str(y), ")")

    result = calculate_absolute_tin_diff_clean_nan_with_mask(std_npy, clean_corner(
        npy[y:y + std_npy.shape[0], x:x + std_npy.shape[1]] - real_bottom, 0, 0), mask, if_show=False,
                                                             target_folder=target_folder, filename=filename, area=area)
    return result, y, x


############################### test function ########################################
def get_tin_result(filepath, target_folder=None):
    """
    检测飞锡
    Args:
        filepath: npy路径

    Returns:

    """
    # start = time.time()
    # 清理边缘
    data = clean_height(filepath)
    # 求pcd板底面深度
    xyz_points = parse_npy_(data)  # 得到x，y，z坐标
    x_points, y_points, z_points = xyz_points
    # show_3d(data)
    # 读取标准件区域
    std_4_3 = np.load(STD_4_3)
    std_4_4 = np.load(STD_4_4)
    std_4_5 = np.load(STD_4_5)

    # 亮度图引入，意图清理无效nan点
    pic = clean_light(filepath[:-4] + "_meta.png")
    pic = cv2.resize(pic, (data.shape[1], data.shape[0]))
    pic = cv2.cvtColor(pic, cv2.COLOR_BGR2GRAY)

    mask43 = np.load("./utils/detection_3d/mask_4-3.npy")
    r43, y43, x43 = calculator_tin_diff_with_mask(x_points, y_points, z_points, std_4_3, data, (AREA_43[2], AREA_43[0]),
                                                  mask43, "43", pic, target_folder=target_folder,
                                                  filename=os.path.basename(filepath))
    mask44 = np.load("./utils/detection_3d/mask_4-4.npy")
    r44, y44, x44 = calculator_tin_diff_with_mask(x_points, y_points, z_points, std_4_4, data, (AREA_44[2], AREA_44[0]),
                                                  mask44, "44", pic, target_folder=target_folder,
                                                  filename=os.path.basename(filepath))
    mask45 = np.load("./utils/detection_3d/mask_4-5.npy")
    r45, y45, x45 = calculator_tin_diff_with_mask(x_points, y_points, z_points, std_4_5, data, (AREA_45[2], AREA_45[0]),
                                                  mask45, "45", pic, target_folder=target_folder,
                                                  filename=os.path.basename(filepath))

    result = ((r43, y43, x43), (r44, y44, x44), (r45, y45, x45))
    # draw_result(target, folder, i, std_4_3.shape, std_4_4.shape, std_4_5.shape, result, data.shape)
    return result


# def write_result(target_file, filename, result):
#     """
#     写入计算结果到csv
#     Args:
#         target_file:
#         filename:
#         result:
#
#     Returns:
#
#     """
#     data = openpyxl.load_workbook(target_file)
#     stn = data.sheetnames[0]
#     table = data.get_sheet_by_name(stn)
#     table = data.active
#     nrows = table.max_row
#     table.cell(nrows + 1, 1).value = filename
#     table.cell(nrows + 1, 2).value = result[0][0]
#     table.cell(nrows + 1, 3).value = result[0][1]
#     table.cell(nrows + 1, 4).value = result[0][2]
#     table.cell(nrows + 1, 5).value = result[1][0]
#     table.cell(nrows + 1, 6).value = result[1][1]
#     table.cell(nrows + 1, 7).value = result[1][2]
#     table.cell(nrows + 1, 8).value = result[2][0]
#     table.cell(nrows + 1, 9).value = result[2][1]
#     table.cell(nrows + 1, 10).value = result[2][2]
#     data.save(target_file)


# def write_result_v2(target_file, filename, result, label_file, n_n, n_p, p_n, p_p, unknown, label_col_input=[2, 3, 4]):
#     """
#     写入计算结果到csv
#     Args:
#         target_file:
#         filename:
#         result:
#         label_file:
#
#     Returns:
#
#     """
#     dmc = filename.split("_")[0]
#     data = openpyxl.load_workbook(target_file)
#     stn = data.sheetnames[0]
#     table = data.get_sheet_by_name(stn)
#     table = data.active
#     nrows = table.max_row
#     table.cell(nrows + 1, 1).value = filename
#     table.cell(nrows + 1, 2).value = result[0][0]
#     table.cell(nrows + 1, 3).value = result[0][1]
#     table.cell(nrows + 1, 4).value = result[0][2]
#     table.cell(nrows + 1, 5).value = result[1][0]
#     table.cell(nrows + 1, 6).value = result[1][1]
#     table.cell(nrows + 1, 7).value = result[1][2]
#     table.cell(nrows + 1, 8).value = result[2][0]
#     table.cell(nrows + 1, 9).value = result[2][1]
#     table.cell(nrows + 1, 10).value = result[2][2]
#     if result[0][0] >= 550:
#         table.cell(nrows + 1, 11).value = 'n'
#     else:
#         table.cell(nrows + 1, 11).value = 'p'
#     # 115 -> 300
#     if result[1][0] >= 300:
#         table.cell(nrows + 1, 13).value = 'n'
#     else:
#         table.cell(nrows + 1, 13).value = 'p'
#     if result[2][0] >= 400:
#         table.cell(nrows + 1, 15).value = 'n'
#     else:
#         table.cell(nrows + 1, 15).value = 'p'
#     label = openpyxl.load_workbook(label_file)
#     labeln = label.sheetnames[0]
#     label_table = label.get_sheet_by_name(labeln)
#     label_table = label.active
#     label_rows = label_table.max_row
#     for label_row in range(1, label_rows + 1):
#         labelname = label_table.cell(label_row, 1).value
#         if (not (labelname is None)) and labelname.split("_")[0] == dmc:
#             table.cell(nrows + 1, 12).value = label_table.cell(label_row, label_col_input[0]).value
#             table.cell(nrows + 1, 14).value = label_table.cell(label_row, label_col_input[1]).value
#             table.cell(nrows + 1, 16).value = label_table.cell(label_row, label_col_input[2]).value
#             break
#     data.save(target_file)
#     if table.cell(nrows + 1, 12).value == 'n':
#         if table.cell(nrows + 1, 11).value == 'n':
#             n_n[0] = n_n[0] + 1
#         elif table.cell(nrows + 1, 11).value == 'p':
#             n_p[0] = n_p[0] + 1
#         else:
#             unknown[0] = unknown[0] + 1
#     elif table.cell(nrows + 1, 12).value == 'p':
#         if table.cell(nrows + 1, 11).value == 'n':
#             p_n[0] = p_n[0] + 1
#         elif table.cell(nrows + 1, 11).value == 'p':
#             p_p[0] = p_p[0] + 1
#         else:
#             unknown[0] = unknown[0] + 1
#     else:
#         unknown[0] = unknown[0] + 1
#     if table.cell(nrows + 1, 14).value == 'n':
#         if table.cell(nrows + 1, 13).value == 'n':
#             n_n[1] = n_n[1] + 1
#         elif table.cell(nrows + 1, 13).value == 'p':
#             n_p[1] = n_p[1] + 1
#         else:
#             unknown[1] = unknown[1] + 1
#     elif table.cell(nrows + 1, 14).value == 'p':
#         if table.cell(nrows + 1, 13).value == 'n':
#             p_n[1] = p_n[1] + 1
#         elif table.cell(nrows + 1, 13).value == 'p':
#             p_p[1] = p_p[1] + 1
#         else:
#             unknown[1] = unknown[1] + 1
#     else:
#         unknown[1] = unknown[1] + 1
#     if table.cell(nrows + 1, 16).value == 'n':
#         if table.cell(nrows + 1, 15).value == 'n':
#             n_n[2] = n_n[2] + 1
#         elif table.cell(nrows + 1, 15).value == 'p':
#             n_p[2] = n_p[2] + 1
#         else:
#             unknown[2] = unknown[2] + 1
#     elif table.cell(nrows + 1, 16).value == 'p':
#         if table.cell(nrows + 1, 15).value == 'n':
#             p_n[2] = p_n[2] + 1
#         elif table.cell(nrows + 1, 15).value == 'p':
#             p_p[2] = p_p[2] + 1
#         else:
#             unknown[2] = unknown[2] + 1
#     else:
#         unknown[2] = unknown[2] + 1
#
#     return n_n, n_p, p_n, p_p, unknown


def draw_result(target_folder, input_folder, filename, std43, std44, std45, point, input_npy_shape):
    """
    根据不同的window找到最合适的区域，并绘制，在calculate_tin_diff中使用
    Args:
        target_folder:
        input_folder:
        filename:
        std43:
        std44:
        std45:
        point: 区域左上角点
        input_npy_shape: npy形状，用于亮度图reshape

    Returns:

    """
    data = clean_light(os.path.join(input_folder, filename[:-4] + "_meta.png"))
    data = cv2.resize(data, (input_npy_shape[1], input_npy_shape[0]))
    # 4-3
    cv2.rectangle(data, (point[0][2], point[0][1]), (point[0][2] + std43[1], point[0][1] + std43[0]), (0, 0, 255), 5)
    # 4-4
    cv2.rectangle(data, (point[1][2], point[1][1]), (point[1][2] + std44[1], point[1][1] + std44[0]), (0, 0, 255), 5)
    # 4-5
    cv2.rectangle(data, (point[2][2], point[2][1]), (point[2][2] + std45[1], point[2][1] + std45[0]), (0, 0, 255), 5)
    print(os.path.join(target_folder, filename[:-4] + "_meta.png"))
    cv2.imwrite(os.path.join(target_folder, filename[:-4] + "_meta.png"), data)


def draw_result_v2(target_folder, input_folder, filename, point):
    """
    根据不同的window找到最合适的区域，并绘制，在calculate_tin_diff中使用
    Args:
        target_folder:
        input_folder:
        filename:
        std43:
        std44:
        std45:
        point: 区域左上角点
        input_npy_shape: npy形状，用于亮度图reshape

    Returns:

    """
    input_npy = clean_height(os.path.join(input_folder, filename))
    data = clean_light(os.path.join(input_folder, filename[:-4] + "_meta.png"))
    data = cv2.resize(data, (input_npy.shape[1], input_npy.shape[0]))
    std43 = np.load(STD_4_3)
    std44 = np.load(STD_4_4)
    std45 = np.load(STD_4_5)
    # 4-3
    cv2.rectangle(data, (point[0][2], point[0][1]), (point[0][2] + std43.shape[1], point[0][1] + std43.shape[0]),
                  (0, 0, 255), 5)
    # 4-4
    cv2.rectangle(data, (point[1][2], point[1][1]), (point[1][2] + std44.shape[1], point[1][1] + std44.shape[0]),
                  (0, 0, 255), 5)
    # 4-5
    cv2.rectangle(data, (point[2][2], point[2][1]), (point[2][2] + std45.shape[1], point[2][1] + std45.shape[0]),
                  (0, 0, 255), 5)
    print(os.path.join(target_folder, filename[:-4] + "_meta.png"))
    cv2.imwrite(os.path.join(target_folder, filename[:-4] + "_meta.png"), data)


def draw_pic(path, target, filename):
    """
    绘制感兴趣区域，预先设定范围，误差较大
    :param path: input folder
    :param target: output folder
    :param filename: input and output filename, image
    :return:
    """
    data = clean_light(os.path.join(path, filename))
    # 4-5
    cv2.rectangle(data, (2030, 10), (2600, 190), (0, 0, 255), 5)
    # 4-4
    cv2.rectangle(data, (2000, 190), (2900, 375), (0, 0, 255), 5)
    # 4-3
    cv2.rectangle(data, (1100, 580), (1610, 820), (0, 0, 255), 5)
    print(os.path.join(target, filename))
    cv2.imwrite(os.path.join(target, filename), data)
    # cv2.imshow("test", data)
    # cv2.waitKey(0)


def check_std(path):
    data = np.load(path)
    show_3d(data)
